#include "aligntarget.hpp"
#include "CoordStack.hpp"
#include <bitset>
#include <utility>

//#include <bitset>

//constexpr int W = 752, H = 480;

//std::bitset<W * H> visited;

//void solve() {
//    // 全部置0
//    visited.reset();
//    // 全部置1
//    visited.set();

//    // 行、列
//    int i, j;

//    // 置1
//    visited.set(i * W + j);
//    visited.set(i * W + j, 1);
//    visited[i * W + j] = 1;

//    // 置0
//    visited.set(i * W + j, 0);
//    visited.reset(i * W + j);
//    visited[i * W + j] = 0;

//    // 查看
//    bool res = visited.test(i * W + j);
//    res = visited[i * W + j];
//}

#include "fmath.hpp"
#include "StaticBuffer.hpp"
#include "fit_quad.hpp"
#include "homography.hpp"
#include "imgA4.hpp"
//#include "bresenham.hpp"
#include "devices.hpp"
extern "C" {
#include "string.h"
}
namespace imgProc {
using namespace apriltag;
namespace align_target {
using std::pair;
#define MAX_SEED_NUM   36
std::bitset<ALIG_W * ALIG_H> dfsrange;//定义记录dfs区域的bit位数
	
	
static inline uint8 is_edge2(uint8 (*img)[ALIG_W],unsigned short int x,unsigned short int y,float thresh) 
{
unsigned short int sum = abs((short) img[y-1][x]-  (short)img[y+1][x])      //上-下
												+abs((short) img[y][x-1]-  (short)img[y][x+1]) 			//左-右
												+abs((short) img[y-1][x-1]-(short)img[y+1][x+1])  //左上-右下
												+abs((short) img[y+1][x-1]-(short)img[y-1][x+1]); //左下-右上
												
unsigned short int T= (short) img[y - 1][x - 1]+  (short) img[y - 1][x] + (short) img[y - 1][x + 1]
										 +(short) img[y][x- 1] +      (short) img[y][x]+      (short) img[y][x + 1]
										 +(short) img[y + 1][x - 1] + (short) img[y + 1][x] + (short) img[y + 1][x + 1];
  if(sum>T/ thresh) return 1; //边界
  else return 0; //非边界
}

void  SobelAutoThreshold2(uint8_t (*imgIn)[ALIG_W], unsigned char (*imgOut)[ALIG_W],float thresh)
{
		for(unsigned short int x=0;x<ALIG_W;x++)
		 {
			 for(unsigned short int y=0;y<ALIG_H;y++)
			 {
				 imgOut[y][x]=is_edge2(imgIn,x,y,thresh)*255;
			 }
		 }
}

//DFS大法
constexpr int dxy8[8][2]{{1, 0}, {1, -1}, {0, -1}, {-1, -1}, {-1, 0}, {-1, 1}, {0, 1}, {1, 1}};
constexpr int dxy4[8][2]{{1, 0}, {0, -1}, {-1, 0}, {0, 1}};

static inline pair<st_doublequad_pixle,int> edgeTrace(unsigned char (*imgIn)[ALIG_W],st_pixle seed ,float thresh) 
{
  int n = 0;
	st_doublequad_pixle vertex8;
	short int p1=ALIG_H,p2=0,p3=ALIG_W,p4=0,p5=ALIG_W+ALIG_H,p6=0,p7=-ALIG_W,p8=ALIG_H;//初始化截距
	CoordStack stack;
	stack.push(seed.x,seed.y);
	dfsrange[seed.y*ALIG_W+seed.x]=1;
	while (!stack.empty()) 
	{
		auto [x, y] = stack.pop();
	
		++n;
		
		
		if(y<p1)    //上
		{
			p1=y;
			vertex8.x[0]=x;
			vertex8.y[0]=y;
		}
		else if(y>p2)//下
		{
			p2=y;
			vertex8.x[1]=x;
			vertex8.y[1]=y;
		}
		if(x<p3)     //左
		{
			p3=x;
			vertex8.x[2]=x;
			vertex8.y[2]=y;
		}
		else if(x>p4)//右
		{
			p4=x;
			vertex8.x[3]=x;
			vertex8.y[3]=y;
		}
	
	
		if(y+x<p5)//上左
		{
			p5=y+x;
			vertex8.x[4]=x;
			vertex8.y[4]=y;
		}
		else if(y+x>p6)//下右
		{
			p6=y+x;
			vertex8.x[5]=x;
			vertex8.y[5]=y;
		}
		if(y-x>p7)//下左
		{
			p7=y-x;
			vertex8.x[6]=x;
			vertex8.y[6]=y;
		}
		else if(y-x<p8)//上右
		{
			p8=y-x;
			vertex8.x[7]=x;
			vertex8.y[7]=y;
		}
		
		for (int i = 0, u, v; i < 8; ++i) 
		{
				u = x + dxy8[i][0], v = y + dxy8[i][1];
				if (u == 0 || u == ALIG_W - 1 || v == 0 || v == ALIG_H - 1) 
				{
					//不进行压栈
				}
				else if(is_edge2(imgIn,u,v,thresh)==1&&dfsrange[v*ALIG_W+u]==0) 
				{
						dfsrange[v*ALIG_W+u] = 1;
						stack.push(u, v);
				}
		}
  }
//	ips200_DrawCircle1(vertex8.x[0]/4,vertex8.y[0]/4+120,5,GREEN);
//	ips200_DrawCircle1(vertex8.x[1]/4,vertex8.y[1]/4+120,5,GREEN);
//	ips200_DrawCircle1(vertex8.x[2]/4,vertex8.y[2]/4+120,5,GREEN);
//	ips200_DrawCircle1(vertex8.x[3]/4,vertex8.y[3]/4+120,5,GREEN);
//	
//	ips200_DrawCircle1(vertex8.x[4]/4,vertex8.y[4]/4+120,6,RED);
//	ips200_DrawCircle1(vertex8.x[5]/4,vertex8.y[5]/4+120,6,RED);
//	ips200_DrawCircle1(vertex8.x[6]/4,vertex8.y[6]/4+120,6,RED);
//	ips200_DrawCircle1(vertex8.x[7]/4,vertex8.y[7]/4+120,6,RED);
  
    return {vertex8,n};
}
//距离计算
float quad_dis(st_pixle_f * A,st_pixle_f * B)
{
	return sqrt((A->x-B->x)*(A->x-B->x)+(A->y-B->y)*(A->y-B->y));
}

//距离计算
float center_dis(st_quad_pixle * A,float cent_x,float cent_y)
{
	return sqrt((A->average_x-cent_x)*(A->average_x-cent_x)+(A->average_y-cent_y)*(A->average_y-cent_y));
}

//逆透视加计算方差
st_quad_pixle homography_quad(const float_t H[3][3],st_doublequad_pixle* doublequad)
{
	st_pixle_f p[8];
	//转换到世界坐标系
	for(int i=0;i<8;i++)
	{
		homography_project(H,doublequad->x[i],doublequad->y[i],&p[i].x,&p[i].y);
	}
	//计算点距
	float d0=quad_dis(&p[0],&p[3]);//上->右
	float d1=quad_dis(&p[3],&p[1]);//右->下
	float d2=quad_dis(&p[1],&p[2]);//下->左
	float d3=quad_dis(&p[2],&p[0]);//左->上
	
	float d4=quad_dis(&p[4],&p[7]);//上边
	float d5=quad_dis(&p[7],&p[5]);//右边
	float d6=quad_dis(&p[5],&p[6]);//下边
	float d7=quad_dis(&p[6],&p[4]);//左边
	//计算方差
	float a1=(d0+d1+d2+d3)/4;
	float a2=(d4+d5+d6+d7)/4;
	float v1=((d0-a1)*(d0-a1)+(d1-a1)*(d1-a1)+(d2-a1)*(d2-a1)+(d3-a1)*(d3-a1))/4;
	float v2=((d4-a2)*(d4-a2)+(d5-a2)*(d5-a2)+(d6-a2)*(d6-a2)+(d7-a2)*(d7-a2))/4;
	
	st_quad_pixle quad;
	if(v1<v2)//方差太大不是正方形
	{

			
		quad.x[0]=p[0].x;quad.y[0]=p[0].y;
		quad.x[1]=p[3].x;quad.y[1]=p[3].y;
		quad.x[2]=p[1].x;quad.y[2]=p[1].y;
		quad.x[3]=p[2].x;quad.y[3]=p[2].y;
		quad.average_x=(p[0].x+p[3].x+p[1].x+p[2].x)/4;
		quad.average_y=(p[0].y+p[3].y+p[1].y+p[2].y)/4;
		quad.average_dis=a1;
		quad.variance=v1;
		return quad;
	}
	else
	{
		quad.x[0]=p[4].x;quad.y[0]=p[4].y;
		quad.x[1]=p[7].x;quad.y[1]=p[7].y;
		quad.x[2]=p[5].x;quad.y[2]=p[5].y;
		quad.x[3]=p[6].x;quad.y[3]=p[6].y;
		quad.average_x=(p[4].x+p[7].x+p[5].x+p[6].x)/4;
		quad.average_y=(p[4].y+p[7].y+p[5].y+p[6].y)/4;
		quad.average_dis=a2;
		quad.variance=v2;
		return quad;
	}
}

//找正方形
uint8_t find_quads(unsigned char (*imgIn)[ALIG_W], float thresh,st_quad_pixle * final_quad,bool visualization=1)
{
	//第一步从中线向四周找出所有种子点并进行记录
	uint16_t seedcntnum=0;
	st_pixle seed_Sequential[MAX_SEED_NUM];
  for (int degree = 0; degree < 360; degree += 10) 
	{
		float rad = degree * Pi / 180;
		float dx,dy;
		
		float x , y ;

		float Angle_divide=atan((float)ALIG_H/(float)ALIG_W)/Pi*180;//角度分割
		if(degree>(180+Angle_divide) && degree<(360-Angle_divide))//上边
		{
			x=ALIG_W/2+cos(rad)/sin(rad)*(ALIG_H/2);//计算起点
			y=1;
			dx =  cos(rad);
			dy = -sin(rad);//生长方向
		}
		else if(degree>Angle_divide && degree<(180-Angle_divide))//下
		{
			x=ALIG_W/2+cos(rad)/sin(rad)*(ALIG_H/2);//计算起点
			y=ALIG_H-2;
			dx = -cos(rad);
			dy = -sin(rad);//生长方向
		}
		else if(degree>(180-Angle_divide)&&degree<(180+Angle_divide))//左
		{
			x=1;
			y=ALIG_H/2+sin(rad)/cos(rad)*(ALIG_W/2);//计算起点
			dx = -cos(rad);
			dy = sin(rad);//生长方向
		}
		else if(degree<Angle_divide || degree>360-Angle_divide)//右
		{
			x=ALIG_W-2;
			y=ALIG_H/2+sin(rad)/cos(rad)*(ALIG_W/2);//计算起点
			dx = -cos(rad);
			dy = -sin(rad);//生长方向
		}
		else
		{
			continue;
		}
		int i, j;
		for (;;) 
		{
			x += dx, y += dy;
			i = y, j = x;
			// ips200_drawpoint(j,i,RED);//画红线
			
			if (i<=0||j<=0||j>=(ALIG_W-1)||i>=(ALIG_H-1))//触碰边界就退出
			{
				break;
			}
			if(is_edge2(imgIn,j, i, thresh)==1)//找到一个种子点就记录
			{
				seed_Sequential[seedcntnum].x=j;
				seed_Sequential[seedcntnum].y=i;
				seedcntnum++;
				if(visualization)ips200_drawpoint(j,i,RED);//ips200_DrawCircle1(j,i,5,RED);//画出种子点
				break;
			}
		}
   }
	// if(visualization)ips200_showint16(188,0,seedcntnum);//ips200_showint16(188,0,seedcntnum);

	//找方块轮廓
	 uint8_t fit_num=0;//记录符合点数限制的dfs区域
	 st_doublequad_pixle quads[20];
	 for(uint16_t i=0;i<seedcntnum;i++)
	 {
		 //第二步按顺序对所有种子点进行dfs首先判断需要dfs的种子点是否在已经dfs的区域内，若在则退出处理下一个种子点，
		 if(dfsrange[seed_Sequential[i].y*ALIG_W+seed_Sequential[i].x]==1)
		 {
			 continue;
		 }
		 else
		 {
		//第三步，若不在则开始dfs并且记录dfs的区域中共两组八个最大边界信息，若dfs结果触碰左右和上边界继续，执行完dfs之后直接退出处理下一个种子点
			auto[quad,dfs_num]=edgeTrace(imgIn,seed_Sequential[i],thresh);
			if(visualization)ips200_showint16(188,1,dfs_num);
			if(dfs_num<1500&&dfs_num>60)//!dfs像素数量约束
			{
				quads[fit_num]=quad;
				++fit_num;
				if(visualization)
				{
					ips200_showint16(188,4,quad.x[7]);   ips200_showint16(250,4,quad.y[7]);
					ips200_showint16(188,5,quad.x[5]);   ips200_showint16(250,5,quad.y[5]);
					ips200_showint16(188,6,quad.x[6]);   ips200_showint16(250,6,quad.y[6]);
					ips200_showint16(188,7,quad.x[4]);   ips200_showint16(250,7,quad.y[4]);
				}
			
			}
			else continue;
		 }
	 }
	 if(visualization) ips200_showint16(188,2,fit_num);


//第四步，透视后计算此dfs区域的两组点距大小，取方差的最小的4个角点
	
    apriltag::float_t quadcorr_arr[4][4]{   // fit_quad_simple所得到的4个角的顺序是固定的
        {135+EXTC, 10,  21, 29.7},  // 右上角
        {152+EXTC, 71, 21, 0,  },  // 右下角
        {90+EXTC, 71, 0,  0,  },  // 左下角
        {89+EXTC, 12.5,  0,  29.7},  // 左上角
    };
		
	apriltag::quad target_quad;
	uint8_t res=0;
	homography_compute2(target_quad.H, quadcorr_arr);//计算单应性矩阵


	st_quad_pixle target_quads_corr[20];
	float final_dis=300;//保存距离中心的距离
	for(int i=0;i<fit_num;i++)
	{
		target_quads_corr[i]=homography_quad(target_quad.H,&quads[i]);
		
		// ips200_showint16(188,4,target_quads_corr[i].x[0]);   ips200_showint16(250,4,target_quads_corr[i].y[0]);
		// ips200_showint16(188,5,target_quads_corr[i].x[1]);   ips200_showint16(250,5,target_quads_corr[i].y[1]);
		// ips200_showint16(188,6,target_quads_corr[i].x[2]);   ips200_showint16(250,6,target_quads_corr[i].y[2]);
		// ips200_showint16(188,7,target_quads_corr[i].x[3]);   ips200_showint16(250,7,target_quads_corr[i].y[3]);

		

		if(target_quads_corr[i].average_dis>=7&&target_quads_corr[i].average_dis<=15&&target_quads_corr[i].variance<50)
		{

			float dis=center_dis(&target_quads_corr[i],0,0);
			if(dis<final_dis)
			{
				final_dis=dis;
				*final_quad=target_quads_corr[i];
			}
			if(visualization)
			{
				ips200_showint16(188,7,target_quads_corr[i].average_x);
				ips200_showint16(188,8,target_quads_corr[i].average_y);
				ips200_showint16(188,9,target_quads_corr[i].average_dis);
				ips200_showint16(188,10,target_quads_corr[i].variance);
			}
			res=1;

		}
	}
//清除
	 	//显示边界
	if(visualization)
	{
		ips200_address_set(0,120,0+188-1,120+120-1);//设置显示区域 
		for(unsigned char j=0;j<120;j++)//H
		{
			for(unsigned char i=0;i<188;i++)//W
			{
				uint16 color = 0;
	      		uint16 temp = 0;
				temp = 255*dfsrange.test(j*188+i);//读取像素点
				color=(0x001f&((temp)>>3))<<11;
				color=color|(((0x003f)&((temp)>>2))<<5);
				color=color|(0x001f&((temp)>>3));
				ips200_wr_data16(color); 
			}
		}
		
	}
	 dfsrange.reset();
		
	
		

 return res;
}




} // namespace edge_detect
} // namespace imgProc